#include<stdio.h>
#include<Arcroll.h>
#include<Mesh_IO/Mesh_IO.h>
#include<Mesh/libcell_iterator.h>

#include<Algorithm/points_cloud_algorithm.h>
#include<poisson_construct.h>
#include<Algorithm/marching_cubes.h>
#include<Matrix/LB_Sparse_Matrix.h>

#include<libcell_macros.h>
//#include<>
#include<IterativeLinearSolvers>
#include<Sparse>
#include<vector>
using namespace Eigen;
typedef Eigen::Triplet<double> T;

extern double fx_dot_fx[5];
extern double fx_dot_Dfx[5];
extern double fx_dot_DDfx[5];


//double fx_dot_fx[5]={1/120.0,13/60.0,11/20.0,13/60.0,1/120.0},fx_dot_Dfx[5]={1/24.0,5/12.0,0, 5/12.0,1/24.0}, fx_dot_DDfx[5]={1/6.0,1/3.0, -1,1/3.0,1/6.0};
Viewer_Something* test_show_grid_points(Viewer_World* vw,Poisson_Grid* pg)
{
    printf("bbegin show grid\n");

    Node* n=vw->create_something(vw,"Points");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Points* vp=(Viewer_Points*)(vs->evolution);
    int sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    sum++;
                }
            }
        }
    }
    vp->Data_rows=sum;
    vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
    vp->set_color(vp,1,0,0,1.0);

   // vp->color=(float*)malloc(sizeof(float)*4*vp->Data_rows);
    //memset(vp->color,1,sizeof(float)*4*vp->Data_rows);
    printf("sum%d\n",sum);
    sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    vp->Data[sum*3+0]=pg->pgps[i][j][k].pos[0];
                    vp->Data[sum*3+1]=pg->pgps[i][j][k].pos[1];
                    vp->Data[sum*3+2]=pg->pgps[i][j][k].pos[2];
                    sum++;
                }
            }
        }
    } 

    vp->pointsize=10.0;
    free_node(n);
    return vs;
}
void  test_show_grid_normal(Viewer_World*vw,Poisson_Grid*pg)
{ 
    Node*n2=vw->create_something(vw,"Edges");
    Viewer_Something* vs=(Viewer_Something*)(n2->value);
    Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
    int sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    sum++;
                }
            }
        }
    } 
    ve->Data_rows=sum*2;
    ve->Data_index_rows=sum;
    ve->color_rows=ve->Data_index_rows;
    ve->Data=(float *)malloc(sizeof(float)*3*ve->Data_rows);    
    ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
    ve->set_color(ve,0,1.0,0.0,1.0);  
    sum=0;
    //printf("data rows:%d index rows:%d\n",ve->Data_rows,ve->Data_index_rows);
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]!=0&&pg->pgps[i][j][k].normal_coeff[1]!=0&&pg->pgps[i][j][k].normal_coeff[2]!=0) 
                {
                    ve->Data[(sum*2)*3+0]=pg->pgps[i][j][k].pos[0];
                    ve->Data[(sum*2)*3+1]=pg->pgps[i][j][k].pos[1];
                    ve->Data[(sum*2)*3+2]=pg->pgps[i][j][k].pos[2];
                    
                    ve->Data[(sum*2+1)*3+0]= ve->Data[(sum*2)*3+0] +pg->pgps[i][j][k].normal_coeff[0]*0.1;
                    ve->Data[(sum*2+1)*3+1]= ve->Data[(sum*2)*3+1] +pg->pgps[i][j][k].normal_coeff[1]*0.1;
                    ve->Data[(sum*2+1)*3+2]= ve->Data[(sum*2)*3+2] +pg->pgps[i][j][k].normal_coeff[2]*0.1;
                    
                    //printf("%lf %lf %lf %lf %lf %lf\n",pg->pgps[i][j][k].pos[0],pg->pgps[i][j][k].pos[1],pg->pgps[i][j][k].pos[2],pg->pgps[i][j][k].normal_coeff[0],pg->pgps[i][j][k].normal_coeff[1],pg->pgps[i][j][k].normal_coeff[2]);
                    ve->Data_index[sum*2]=sum*2;
                    ve->Data_index[sum*2+1]=sum*2+1; 
                    sum++;
                    //vp->Data[sum*3+0]=pg->pgps ; 
                }
            }
        }
    }  
    free_node(n2);
}
Viewer_Something* test_show_mesh_points(Viewer_World* vw,Mesh* m)
{
    Node* n=vw->create_something(vw,"Points");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Points* vp=(Viewer_Points*)(vs->evolution);
    vp->Data_rows=m->num_v(m);
    vp->Data=(float*)malloc(sizeof(float)*3*vp->Data_rows);
    int i=0;
    vp->color=(float*)malloc(sizeof(float)*4*vp->Data_rows);
    //memset(vp->color,1,sizeof(float)*4*vp->Data_rows);    
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        for(int j=0;j<3;j++)
        {
            vp->Data[i*3+j]=quote(vit)->point[j];
        }   
        vp->color[i*4+0]=1.0;vp->color[i*4+1]=0.8;vp->color[i*4+2]=0.0;vp->color[i*4+3]=1.0;
        i++;
    } 
    vp->pointsize=10.0;
    free_node(n);
    return vs;
}
void test_show_normal(Viewer_World* vw,Mesh* m)
{
    Node*n2=vw->create_something(vw,"Edges");
    Viewer_Something* vs=(Viewer_Something*)(n2->value);
    Viewer_Edges* ve=(Viewer_Edges*)(vs->evolution);
    ve->Data_rows=m->num_v(m)*2;
    ve->Data_index_rows=m->num_v(m);
    ve->color_rows=ve->Data_index_rows;
    ve->Data=(float *)malloc(sizeof(float)*3*ve->Data_rows);    
    ve->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*2*ve->Data_index_rows);
    ve->color=(float*)malloc(sizeof(float)*4*ve->color_rows);
    ve->set_color(ve,1.0,0.0,0.0,1.0);  
    int i=0;
    printf("data rows:%d index rows:%d\n",ve->Data_rows,ve->Data_index_rows);
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        for(int j=0;j<3;j++)
        {
            ve->Data[(i*2)*3+j]=quote(vit)->point[j];
        }
        double*normal=(double*)(quote(vit)->prop);
        for(int j=0;j<3;j++)
        {
            ve->Data[(i*2+1)*3+j]=quote(vit)->point[j]+normal[j]*0.1;
        }   
        ve->Data_index[i*2]=i*2;
        ve->Data_index[i*2+1]=i*2+1;

        i++;
    }
    free_node(n2);
}

void fill_normal_coeff(Poisson_Grid*pg,Node* node_vertices)
{
    int i=0,j=0,k=0;
    double* normal=NULL;
    for(Node* nit=node_vertices;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        i=(int)((v->point[0]+pg->range)/pg->width);
        j=(int)((v->point[1]+pg->range)/pg->width);
        k=(int)((v->point[2]+pg->range)/pg->width);
        i=(i<0?0:(i>=pg->size? (pg->size-1): i ));
        j=(j<0?0:(j>=pg->size? (pg->size-1): j )); 
        k=(k<0?0:(k>=pg->size? (pg->size-1): k ));
        if(pg->pgps[i][j][k].normal_coeff[0]==0&&pg->pgps[i][j][k].normal_coeff[1]==0&&pg->pgps[i][j][k].normal_coeff[2]==0)
        {
            normal=(double*)(v->prop);
            pg->pgps[i][j][k].normal_coeff[0]=normal[0];
            pg->pgps[i][j][k].normal_coeff[1]=normal[1];
            pg->pgps[i][j][k].normal_coeff[2]=normal[2];
        } 
    }
}
void lb_sparse_matrix_insert1(LB_Sparse_Matrix*lsm,int i,int j,double data)
{
    LB_Sparse_Matrix_Node* p=(LB_Sparse_Matrix_Node*)malloc(sizeof(LB_Sparse_Matrix_Node));
    p->i=i;p->j=j;p->data=data;p->right=NULL;p->down=NULL;
    LB_Sparse_Matrix_Node*q=NULL; 
    if(NULL==lsm->rhead[i]||lsm->rhead[i]->j>j)
    {
        p->right=lsm->rhead[i];
        lsm->rhead[i]=p;
    } 
    else
    {
        for(q=lsm->rhead[i];(q->right)&&q->right->j<j;q=q->right); 
        if(q->right&&q->right->j==j)
        {
            free(p);
            q->right->data=data;
            return ;
        }
        else
        {
            p->right=q->right;
            q->right=p;
        }
    }

    if(NULL==lsm->chead[j]||lsm->chead[j]->i>i)
    {
        p->down=lsm->chead[j];
        lsm->chead[j]=p;
    }
    else
    {
        for(q=lsm->chead[j];(q->down)&&q->down->i<i;q=q->down);
        if(q->down&&q->down->i==i)
        {
            free(p);
            q->down->data=data;
        }
        else
        {
            p->down=q->down;
            q->down=p;
        } 
    }
}

void  poisson_test_sparse_matrix1(Poisson_Grid*pg)
{
    printf("begein sparse matrix1\n");

    int len=pg->size*pg->size*pg->size;
    LB_Sparse_Matrix*lsm=(LB_Sparse_Matrix*)malloc(sizeof(LB_Sparse_Matrix));
    lb_sparse_matrix_initn(lsm,len,len,0);
    int id=0,id1=0;
    int sum=0;double temp=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {

            for(int k=0;k<pg->size;k++)
            {

                id=threeD_index2_id(i,j,k,pg->size);
                sum++;
                //printf("herer\n");

                for(int l=0;l<5;l++ )
                {
                    for(int m=0;m<5;m++)
                    {
                        for(int n=0;n<5;n++)
                        {
                            if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size)
                            {
                                continue;
                            }
                            id1=threeD_index2_id(i+l-2,j+m-2,k+n-2,pg->size);

                            temp=0;
                            temp+=(fx_dot_DDfx[l]*fx_dot_fx[m]*fx_dot_fx[n]); 
                            temp+=(fx_dot_fx[l]*fx_dot_DDfx[m]*fx_dot_fx[n]); 
                            temp+=(fx_dot_fx[l]*fx_dot_fx[m]*fx_dot_DDfx[n]);
                            temp/=(pg->width*pg->width);
                            lb_sparse_matrix_insert(lsm,id,id1,temp);
                            //printf("once\n");
                            //A.insert(id,id1)=temp; 
                            // tripletList.push_back(T(id,id1,temp));     
                        }
                    }
                }
            }
        }
    }
    printf("end fill A\n");

    double*bi= poisson_compute_b(pg );
    printf("conpute gonju\n");
    pg->solve=compute_conjugate_gradient(lsm,bi);
    printf("end con\n");
    
    free(bi);


    lb_sparse_matrix_free(lsm);

}
void  poisson_test_sparse_matrix(Poisson_Grid*pg )
{

    int len=pg->size*pg->size*pg->size;
    SparseMatrix<double> A(len,len);
    
    //A.setZero();
    A.reserve(VectorXi::Constant(pg->size*pg->size*pg->size,150));
    //std::vector<T> tripletList;
    //tripletList.reserve(pg->size*pg->size*pg->size*140);
    printf("begin sparse matrix\n");
    int id=0,id1=0;
    int sum=0;double temp=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {

            for(int k=0;k<pg->size;k++)
            {

                id=threeD_index2_id(i,j,k,pg->size);
                sum++;
                //printf("herer\n");

                for(int l=0;l<5;l++ )
                {
                    for(int m=0;m<5;m++)
                    {
                        for(int n=0;n<5;n++)
                        {
                            if(i+l-2<0||i+l-2>=pg->size||j+m-2<0||j+m-2>=pg->size||k+n-2<0||k+n-2>=pg->size)
                            {
                                continue;
                            }
                            id1=threeD_index2_id(i+l-2,j+m-2,k+n-2,pg->size);

                            temp=0;
                            temp+=(fx_dot_DDfx[l]*fx_dot_fx[m]*fx_dot_fx[n]); 
                            temp+=(fx_dot_fx[l]*fx_dot_DDfx[m]*fx_dot_fx[n]); 
                            temp+=(fx_dot_fx[l]*fx_dot_fx[m]*fx_dot_DDfx[n]);
                            temp/=(pg->width*pg->width);
                            //A.insert(id,id1)=temp;
                            A.coeffRef(id,id1)=temp; 
                            // tripletList.push_back(T(id,id1,temp));     
                        }
                    }
                }
            }
        }
    }
   // A.setFromTriplets(tripletList.begin(),tripletList.end());

    printf("beigin compress\n");

    A.makeCompressed();

    ConjugateGradient<SparseMatrix<double>,Eigen::Upper> solver;
  
    double*bi= poisson_compute_b(pg );

    Eigen::VectorXd b(len);
    for(int i=0;i<len;i++)
    {
        b.coeffRef(i)=bi[i];
       
    }   


    printf("end b\n"); 

    free(bi);
      
    VectorXd x= solver.compute(A).solve(b); 
    pg->solve=(double*)malloc(sizeof(double)*len); 
    for(int i=0;i<len;i++)
    {
        pg->solve[i]=x.coeffRef(i);
      //  if(bi[i]!=0)
        // 
        //printf("%lf \n",x.coeffRef(i));

    }
    // VectorXd y=A* x;
    // for(Node* nit=ids;nit!=NULL;nit=(Node*)(nit->Next))
    // {
    //     SAFE_FREE(nit->value);
    //     //free(nit->value);nit->value=NULL;
    // }
    //free_node_value(ids);
    // free_node(ids); 
    printf(" 1sum:%d\n",sum);
}


double* test_grid_normal_fun(double *p,Poisson_Grid*pg)
{
    double *re=(double*)malloc(sizeof(double)*3);
    memset(re,0,sizeof(double)*3);
    int i=(int)((p[0]+pg->range)/pg->width);
    int j=(int)((p[1]+pg->range)/pg->width);
    int k=(int)((p[2]+pg->range)/pg->width);
    printf("i j k:%d %d %d\n",i,j,k);
    for(int l=0;l<3;l++)
    {
        for(int m=0;m<3;m++)
        {
            for(int n=0;n<3;n++)
            {
                if(i+l-1<0||i+l-1>=pg->size||j+m-1<0||j+m-1>=pg->size||k+n-1<0||k+n-1>=pg->size)
                {
                    continue;
                }
                re[0]+=(poisson_FF_deformation(pg->pgps[i+l-1][j+m-1][k+n-1].pos,pg->width,p)*pg->pgps[i+l-1][j+m-1][k+n-1].normal_coeff[0]); 
                re[1]+=(poisson_FF_deformation(pg->pgps[i+l-1][j+m-1][k+n-1].pos,pg->width,p)*pg->pgps[i+l-1][j+m-1][k+n-1].normal_coeff[1]);
                re[2]+=(poisson_FF_deformation(pg->pgps[i+l-1][j+m-1][k+n-1].pos,pg->width,p)*pg->pgps[i+l-1][j+m-1][k+n-1].normal_coeff[2]); 
            }
        } 
    } 
    return re;
}



static inline double alpha_test(double x)
{
    return (x-1-fabs(x-1))/2.0;
}

static inline double my_fun1(double *p,void * tool)
{
    double vv1=p[0]*p[0]+p[1]*p[1]+p[2]*p[2];
    double vv2=(p[0]-0.5)*(p[0]-0.5)+(p[1]-0.5)*(p[1]-0.5)+(p[2]-0.5)*(p[2]-0.5);

    return alpha_test(vv1)+alpha_test(vv2)+1.0; 
}

static inline double my_fun2(double*p,void* tool)
{
    return p[0]*p[0]+p[1]*p[1]+p[2]*p[2];
}
double my_fun3(double* p,void * tool)
{
    double re=0;
    Poisson_Grid* pg=(Poisson_Grid*)(tool);
    int i=(int)((p[0]+pg->range)/pg->width);
    int j=(int)((p[1]+pg->range)/pg->width);
    int k=(int)((p[2]+pg->range)/pg->width);
    int id=0;
    for(int l=0;l<3;l++)
    {
        for(int m=0;m<3;m++)
        {
            for(int n=0;n<3;n++)
            {
                if(i+l-1<0||i+l-1>=pg->size||j+m-1<0||j+m-1>=pg->size||k+n-1<0||k+n-1>=pg->size)
                {
                    continue;
                }
                id=threeD_index2_id(i+l-1,j+m-1,k+n-1,pg->size);
                re+=(poisson_FF_deformation(pg->pgps[i+l-1][j+m-1][k+n-1].pos,pg->width,p)*pg->solve[id]); 
            }
        } 
    }  

    return re;
}


void test_show_triangles(Viewer_World* vw, Node* node_tri)
{
    Node* n=vw->create_something(vw,"Faces");
    Viewer_Something*vs=(Viewer_Something*)(n->value);
    Viewer_Faces* vf=(Viewer_Faces*)(vs->evolution);
     
    vf->Data_rows=node_size(node_tri)*3;
    vf->Data_index_rows=node_size(node_tri);
    printf("node_tr size:%d\n",vf->Data_rows);

    vf->Data=(float*)malloc(sizeof(float)*3*vf->Data_rows);
    vf->Data_index=(unsigned int*)malloc(sizeof(unsigned int)*4*vf->Data_index_rows);
    vf->normal_rows=vf->Data_index_rows;
    vf->normal=(float*)malloc(sizeof(float)*3*vf->normal_rows);
    memset(vf->normal,0,sizeof(float)*3*vf->normal_rows);
    int i=0;
    for(Node* nit=node_tri;nit!=NULL;nit=(Node*)(nit->Next))
    {
        TRIANGLE* triangle=(TRIANGLE*)(nit->value);
        normalize(triangle->normals[0],3);
        for(int j=0;j<3;j++)
        {
            for(int k=0;k<3;k++)
            {
                vf->Data[i*9+j*3+k]=triangle->p[j][k];  
            }
            vf->normal[i*3+j]=triangle->normals[0][j];
        }
        vf->Data_index[i*4+0]=3; vf->Data_index[i*4+1]=i*3+0;vf->Data_index[i*4+2]=i*3+1;vf->Data_index[i*4+3]=i*3+2;
        i++;
    } 
    vf->color_rows=vf->Data_index_rows;
    vf->set_color(vf,0.9,0.6,0.7,1.0);
    free_node(n); 
}
double test_poisson_solve_fun(Poisson_Grid* pg)
{
    double test_iso=0;int sum=0;
    for(int i=0;i<pg->size;i++)
    {
        for(int j=0;j<pg->size;j++)
        {
            for(int k=0;k<pg->size;k++)
            {
                if(pg->pgps[i][j][k].normal_coeff[0]==0&&pg->pgps[i][j][k].normal_coeff[1]==0&&pg->pgps[i][j][k].normal_coeff[2]==0) 
                {

                    continue; 
                }
                test_iso+=my_fun3(pg->pgps[i][j][k].pos,pg);
                sum++;
             //  printf("%lf ",my_fun3(pg->pgps[i][j][k].pos,pg));
            }
        }
    } 
    printf("test_iso:%lf\n",test_iso/(double)(sum));
    return test_iso/((double)sum);
}


void test()
{
    printf("begin test fun \n");
    Mesh mesh;
    Mesh_init(&mesh);
    _ReadOff_(&mesh,"rabbitCut.off",3);

    mesh_adjust_mesh_to_unit_hexahedron(&mesh); 

    template_v ** values=(template_v**)malloc(sizeof(template_v*)*mesh.num_v(&mesh));
    int i=0;
    Node* node_vertices=NULL;
    for(auto vit=mesh.v_begin(&mesh);vit!=mesh.v_end(&mesh);vit++ )
    {
        values[i]=quote(vit);
        node_vertices=node_overlying(node_vertices,quote(vit));
        i++;
    }

    KD_Node*kdtree1= create_kd_tree(values,i,0); 
    
    points_cloud_compute_normal_using_clapack(node_vertices,kdtree1,7);
    double r=compute_visual_avarage_distance(node_vertices);
    adjust_normal_of_node_vertices(node_vertices,r,kdtree1);

    Poisson_Grid*pg=(Poisson_Grid*)malloc(sizeof(Poisson_Grid));
    poisson_grid_init(pg);

    poisson_grid_init_with_depth(pg,8); 

    fill_normal_coeff(pg,node_vertices);
    // double test_x[3]={0.436281,0.242219,-0.517031};
    // double*test_y=test_grid_normal_fun(test_x,pg);
    // normalize(test_y,3);
    // printf("test_y:%lf %lf %lf\n",test_y[0],test_y[1],test_y[2]);
    // free(test_y);
  //  printf("size :%d\n",pg->size);
    //double*bi= poisson_compute_b(pg);
    //poisson_test_sparse_matrix1(pg);
    poisson_test_sparse_matrix(pg);

    double iso=test_poisson_solve_fun(pg);
    //Node* node_tris=marching_cubes_generate_triangles(0.2,100,my_fun2,NULL);
    Node* node_tris=marching_cubes_generate_triangles(iso,100,my_fun3,pg);

 
/// visual


    Viewer_World_Manager vwm;
    viewer_world_manager_init(&vwm);
    Viewer_World *vw=vwm.create_world(&vwm,NULL);

    add_default_somethings(vw); 
    // test_show_mesh_points(vw,&mesh);
    // test_show_normal(vw,&mesh);
    // test_show_grid_points(vw,pg);
    // test_show_grid_normal(vw,pg);

    test_show_triangles(vw,node_tris);

    Viewer_Opengl_Interpreter voi;
    viewer_opengl_interpreter_initn(&voi,&vwm);
    voi.interpreter(&voi);


    viewer_opengl_interpreter_free(&voi);
    viewer_world_manager_free(&vwm);
// visual 
 
    poisson_grid_free(pg);
 
    free(values);
    free_node(node_vertices);
    free_kdnode(kdtree1); 

    Mesh_free(&mesh);
}


int main()
{
    test();


    printf("end\n");
    return 0;
}